/**************************************************************************
 *                                                                         *
 * ADXL345 Driver for Arduino                                              *
 *                                                                         *
 ***************************************************************************
 *                                                                         * 
 * This program is free software; you can redistribute it and/or modify    *
 * it under the terms of the GNU License.                                  *
 * This program is distributed in the hope that it will be useful,         *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of          *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
 * GNU License V2 for more details.                                        *
 *                                                                         *
 ***************************************************************************/
#include <includes.h>
#include "utype.h"
#include "I2CRoutines.h"
#include "MyRtos.h"
#include "Modbus.h"
#include "ADXL345.h"


#define ADXL345_DEVICE (0x53)    // ADXL345 device address
#define ADXL345_TO_READ (6)      // num of bytes we are going to read each time (two bytes for each axis)

#define  B00000011    0x3
#define  B00000000    0
#define  B00000001    1
#define  B00000010    2
#define  B11101100    0xec
#define  B00001111    0xf
#define  B11110000    0xf0


void ADXL345::init(void) {
	status = ADXL345_OK;
	error_code = ADXL345_NO_ERROR;
	
	gains[0] = 0.00376390;
	gains[1] = 0.00376009;
	gains[2] = 0.00349265;
}

void ADXL345::powerOn(void) {
	//Turning on the ADXL345
	writeTo(ADXL345_POWER_CTL, 0);      
	writeTo(ADXL345_POWER_CTL, 16);
	writeTo(ADXL345_POWER_CTL, 8);
}

// Reads the acceleration into three variable x, y and z
void ADXL345::readAccel(int *xyz){ 
	readAccel(xyz, xyz + 1, xyz + 2);        
}
void ADXL345::readAccel(int *x, int *y, int *z) {  
	readFrom(ADXL345_DATAX0, ADXL345_TO_READ, _buff); //read the acceleration data from the ADXL345
	// each axis reading comes in 10 bit resolution, ie 2 bytes.  Least Significat Byte first!!
	// thus we are converting both bytes in to one int
	*x = (((int)_buff[1]) << 8) | _buff[0];   
	*y = (((int)_buff[3]) << 8) | _buff[2];
	*z = (((int)_buff[5]) << 8) | _buff[4];
}

void ADXL345::get_Gxyz(double *xyz){ 
	int i;
	int xyz_int[3];
	readAccel(xyz_int);
	for(i=0; i<3; i++){
		xyz[i] = xyz_int[i] * gains[i];
	}
}
// Writes val to address register on device
void ADXL345::writeTo(byte address, byte val) {
  _buff[0] = address;
  _buff[1] = val;
  // Wait I2C2 free
  WaitI2C2();  
  I2C2_Master_BufferWrite(_buff,2, ADXL345_DEVICE);
  FreeI2C2();   
}

// Reads num bytes starting from address register on device in to _buff array
void ADXL345::readFrom(byte address, int num, byte _buff[]) {
  WaitI2C2();
  if(I2C2_Read_Registers(ADXL345_DEVICE,address,_buff, num ) != Success) {
    status = ADXL345_ERROR;
    error_code = ADXL345_READ_ERROR;
  } 
  FreeI2C2();
}


// Gets the range setting and return it into rangeSetting
// it can be 2, 4, 8 or 16
void ADXL345::getRangeSetting(byte* rangeSetting) {
	byte _b;
	readFrom(ADXL345_DATA_FORMAT, 1, &_b);
	*rangeSetting = _b & B00000011;
}

// Sets the range setting, possible values are: 2, 4, 8, 16
void ADXL345::setRangeSetting(int val) {
	byte _s;
	byte _b;
	
	switch (val) {
		case 2:  
			_s = B00000000; 
			break;
		case 4:  
			_s = B00000001; 
			break;
		case 8:  
			_s = B00000010; 
			break;
		case 16: 
			_s = B00000011; 
			break;
		default: 
			_s = B00000000;
	}
	readFrom(ADXL345_DATA_FORMAT, 1, &_b);
	_s |= (_b & B11101100);
	writeTo(ADXL345_DATA_FORMAT, _s);
}
// gets the state of the SELF_TEST bit
bool ADXL345::getSelfTestBit() {
	return getRegisterBit(ADXL345_DATA_FORMAT, 7);
}

// Sets the SELF-TEST bit
// if set to 1 it applies a self-test force to the sensor causing a shift in the output data
// if set to 0 it disables the self-test force
void ADXL345::setSelfTestBit(bool selfTestBit) {
	setRegisterBit(ADXL345_DATA_FORMAT, 7, selfTestBit);
}

// Gets the state of the FULL_RES bit
bool ADXL345::getFullResBit() {
	return getRegisterBit(ADXL345_DATA_FORMAT, 3);
}

// Sets the FULL_RES bit
// if set to 1, the device is in full resolution mode, where the output resolution increases with the
//   g range set by the range bits to maintain a 4mg/LSB scal factor
// if set to 0, the device is in 10-bit mode, and the range buts determine the maximum g range
//   and scale factor
void ADXL345::setFullResBit(bool fullResBit) {
	setRegisterBit(ADXL345_DATA_FORMAT, 3, fullResBit);
}

// Gets the state of the justify bit
bool ADXL345::getJustifyBit() {
	return getRegisterBit(ADXL345_DATA_FORMAT, 2);
}

// Sets the JUSTIFY bit
// if sets to 1 selects the left justified mode
// if sets to 0 selects right justified mode with sign extension
void ADXL345::setJustifyBit(bool justifyBit) {
	setRegisterBit(ADXL345_DATA_FORMAT, 2, justifyBit);
}

// set/get the gain for each axis in Gs / count
void ADXL345::setAxisGains(double *_gains){
	int i;
	for(i = 0; i < 3; i++){
		gains[i] = _gains[i];
	}
}
void ADXL345::getAxisGains(double *_gains){
	int i;
	for(i = 0; i < 3; i++){
		_gains[i] = gains[i];
	}
}


// Sets the OFSX, OFSY and OFSZ bytes
// OFSX, OFSY and OFSZ are user offset adjustments in twos complement format with
// a scale factor of 15,6mg/LSB
// OFSX, OFSY and OFSZ should be comprised between 
void ADXL345::setAxisOffset(int x, int y, int z) {
	writeTo(ADXL345_OFSX, byte (x));  
	writeTo(ADXL345_OFSY, byte (y));  
	writeTo(ADXL345_OFSZ, byte (z));  
}

// Gets the OFSX, OFSY and OFSZ bytes
void ADXL345::getAxisOffset(int* x, int* y, int*z) {
	byte _b;
	readFrom(ADXL345_OFSX, 1, &_b);  
	*x = int (_b);
	readFrom(ADXL345_OFSY, 1, &_b);  
	*y = int (_b);
	readFrom(ADXL345_OFSZ, 1, &_b);  
	*z = int (_b);
}


bool ADXL345::isAsleep(){ 
	return getRegisterBit(ADXL345_ACT_TAP_STATUS, 3); 
}

bool ADXL345::isLowPower(){ 
	return getRegisterBit(ADXL345_BW_RATE, 4); 
}
void ADXL345::setLowPower(bool state) {  
	setRegisterBit(ADXL345_BW_RATE, 4, state); 
}

double ADXL345::getRate(){
	byte _b;
	readFrom(ADXL345_BW_RATE, 1, &_b);
	_b &= B00001111;
	return (pow(2.0f,((int) _b)-6)) * 6.25;
}

void ADXL345::setRate(double rate){
	byte _b,_s;
	int v = (int) (rate / 6.25);
	int r = 0;
	while (v >>= 1)
	{
		r++;
	}
	if (r <= 9) { 
		readFrom(ADXL345_BW_RATE, 1, &_b);
		_s = (byte) (r + 6) | (_b & B11110000);
		writeTo(ADXL345_BW_RATE, _s);
	}
}

void ADXL345::set_bw(byte bw_code){
	if((bw_code < ADXL345_BW_3) || (bw_code > ADXL345_BW_1600)){
		status = false;
		error_code = ADXL345_BAD_ARG;
	}
	else{
		writeTo(ADXL345_BW_RATE, bw_code);
	}
}

byte ADXL345::get_bw_code(){
	byte bw_code;
	readFrom(ADXL345_BW_RATE, 1, &bw_code);
	return bw_code;
}





//Used to check if action was triggered in interrupts
//Example triggered(interrupts, ADXL345_SINGLE_TAP);
bool ADXL345::triggered(byte interrupts, int mask){
	return ((interrupts >> mask) & 1);
}


void ADXL345::setInterrupt(byte interruptBit, bool state) {
	setRegisterBit(ADXL345_INT_ENABLE, interruptBit, state);
}

void ADXL345::setRegisterBit(byte regAdress, int bitPos, bool state) {
	byte _b;
	readFrom(regAdress, 1, &_b);
	if (state) {
		_b |= (1 << bitPos);  // forces nth bit of _b to be 1.  all other bits left alone.
	} 
	else {
		_b &= ~(1 << bitPos); // forces nth bit of _b to be 0.  all other bits left alone.
	}
	writeTo(regAdress, _b);  
}

bool ADXL345::getRegisterBit(byte regAdress, int bitPos) {
	byte _b;
	readFrom(regAdress, 1, &_b);
	return ((_b >> bitPos) & 1);
}
